#include "BSP_can.h"
#include "struct_typedef.h"
#include "main.h"
#include "cmsis_os.h"
#include "stm32f1xx_hal.h"

extern CAN_HandleTypeDef hcan;


/********************通用***************************/

/**********************双板时（底盘）使用********************************/		
//获取UI数据		
#define get_ui_measure(ptr, data)                      		 		 			  \
{    int16_t i = 0;          																					\
	   for(i=0;i<2;i++)                                                 \
	   {																																\
			 (ptr)->speed_set.s[i] = data[i];															\
		 }																																\
		(ptr)->reset = data[2];																						\
	  (ptr)->IfFirc = data[3];																						\
		(ptr)->IfShift = data[4];																						\
		(ptr)->Auto = data[5];																						\
		(ptr)->Spin = data[6];																						\
}
/**********************双板时（底盘）使用********************************/		
//获取vx_set，vy_Set
#define get_gimabl_measure(ptr, data)                      		 	  \
{   int16_t i =0;                                                     \
		for(i=0;i<4;i++)																								 	\
		{																																	\
			(ptr)->vx_set.s[i]= data[i];																			\
			(ptr)->vy_set.s[i]= data[i+4];																		\
		}																																	\
}

/**********************双板时（底盘）使用********************************/		
//获取wz_set
#define get_wz_measure(ptr, data)                      		 	 		  \
{   int16_t i =0;                                                     \
		for(i=0;i<4;i++)																								 	\
		{																																	\
			(ptr)->wz_set.s[i]= data[i+4];			\
		}		                                        \
		(ptr)->if_open=data[0];		  \
		(ptr)->mode=data[1]	;\
		(ptr)->id=data[2];	\
}
//				for(i=0;i<4;i++)																								 	\
//				{																																	\
//				 (ptr)->pitch.s[i]= data[i];																			\
//				}			

/**********************双板时（底盘）使用********************************/	
//获取yaw,pitch角度值
#define get_angle_info(ptr, data)																	\
		{		int16_t i =0;																											\
			  for(i=0;i<4;i++)																								 	\
				{																																	\
				 (ptr)->yaw_angle.s[i]= data[i];																	\
				 (ptr)->pitch_angle.s[i]= data[i+4];															\
				}																																	\
		}
/**********************双板时（底盘）使用********************************/		
//获取x,y坐标		
#define get_x_y_info(ptr, data)																	\
		{		int16_t i =0;																											\
			  for(i=0;i<4;i++)																								 	\
				{																																	\
				 (ptr)->x_coordinate.s[i]= data[i];																	\
				 (ptr)->y_coordinate.s[i]= data[i+4];															\
				}																																	\
		}
/**********************双板时（底盘）使用********************************/			
//获取预测x,y坐标		
		#define get_pre_x_y_info(ptr, data)																	\
		{		int16_t i =0;																											\
			  for(i=0;i<4;i++)																								 	\
				{																																	\
				 (ptr)->pre_x_coordinate.s[i]= data[i];																	\
				 (ptr)->pre_y_coordinate.s[i]= data[i+4];															\
				}																																	\
		}
		
/**********************双板时（底盘）使用********************************/			
//获取预测x,y坐标		
		#define get_r_info(ptr, data)																	\
		{		int16_t i =0;																											\
			  for(i=0;i<4;i++)																								 	\
				{																																	\
				 (ptr)->radius.s[i]= data[i];																	\
				}																																	\
				(ptr)->vision_mode = data[4];                                     \
		}		
/**********************双板时（云台）使用********************************/		
#define get_revolver_limit_measure(ptr, data)                   \
{                                                                   \
  (ptr)->if_shoot = (uint8_t)(data)[0] ;       										\
}

/**********************双板时（云台）使用********************************/		
#define get_bullet_speed_measure(ptr, data)														\
{   int16_t i =0;                                                     \
		for(i=0;i<4;i++)																								 	\
		{																																	\
			(ptr)->s[i]= data[i];																						\
		}																																	\
}

/**********************双板时（云台）使用********************************/	
#define gimbal_get_cap_measure(ptr, data)                                  \
{                                                                   \
 (ptr)->power = (uint16_t)((data)[0] << 8 | (data)[1]);          \
 (ptr)->voltage = (uint16_t)((data)[2] << 8 | (data)[3]);      	\
 (ptr)->current = (uint16_t)((data)[4] << 8 | (data)[5]);  			\
}

//浙纺超电
#define get_robofuture_cap_measure(ptr, data)			\
{																									\
	(ptr)->warning = data[0];           \
	(ptr)->DischargeStop = (data)[1];     \
	(ptr)->chassis_power = (uint16_t)((data)[2] << 8 | (data)[3]); \
	(ptr)->ele_quantity = (uint16_t)((data)[4] << 8 | (data)[5]); \
}

// #define get_mpu6050_measure(ptr,data)   \
// {                 
//     int16_t i =0;                      \                                                    
//     for (i = 0;i<4;i++)
//     {                                                     
//         (ptr)->accel_pitch.s[i] = data[i];               \
//         (ptr)->accel_roll.s[i] = data[i+4];                \
//         (ptr)->pitch_angle.s[i] = data[i+8];                \
//         (ptr)->roll_angle.s[i] = data[i+12];                 \
//     }                                                 

// }
/********************CAN线发送的结构体定义***************************/
static CAN_TxHeaderTypeDef  CAN1_200_tx_message;
static uint8_t              CAN1_200_send_data[8];
static CAN_TxHeaderTypeDef  CAN1_1FF_tx_message;
static uint8_t              CAN1_1FF_send_data[8];
static CAN_TxHeaderTypeDef  CAN2_200_tx_message;
static uint8_t              CAN2_200_send_data[8];
static CAN_TxHeaderTypeDef  CAN2_1FF_tx_message;
static uint8_t              CAN2_1FF_send_data[8];
static CAN_TxHeaderTypeDef  Reset_tx_message;
static uint8_t              Reset_tx_send_data[8];
/*****************发送特定信息专用*************************/
static CAN_TxHeaderTypeDef CAN1_301_tx_message;
static uint8_t              CAN1_301_send_data[8];
static CAN_TxHeaderTypeDef CAN1_3FF_tx_message;
static uint8_t              CAN1_3FF_send_data[8];
static CAN_TxHeaderTypeDef  chassis_tx_message;
static uint8_t              chassis_can_send_data[8];	
// static CAN_TxHeaderTypeDef CAN1_306_tx_message;
// static uint8_t              CAN1_306_send_data[8];
static CAN_TxHeaderTypeDef CAN1_208_tx_message;
static uint8_t              CAN1_208_send_data[8];

/*************上下双板时（云台）使用**************/
Chassis_Order chassis_order_vx;
Chassis_Order chassis_order_vy;
Chassis_Order chassis_order_wz;
Yaw_Pitch yaw;
Yaw_Pitch pitch;
static chassis_to_gimbal_t    revolver_limit;	
//static Chassis_Order bullet_speed;

fp32 speed_offest;
bool_t if_speed_update;
/*************上下双板时（底盘）使用**************/
UI_order_to_chassis UI_Measure;
static chassis_order_measure_t chassis_order_measure;
Shoot shoot_speed;

/********************通用***************************/
Chassis_Power_t		cap_measure;
robofuture_cap_measure_t robo_cap_measure;
mpu6050_send_data_t mpu6050_send_data;
//统一处理can接收函数
static void CAN_hook(CAN_RxHeaderTypeDef*rx_message , uint8_t* data);
/**
  * @brief          CAN线过滤器初始化
  * @param[in]      Null
  * @retval         Null
  */
void can_filter_init(void)
{

    CAN_FilterTypeDef can_filter_st;
    can_filter_st.FilterActivation = ENABLE;
    can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
    can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
    can_filter_st.FilterIdHigh = 0x0000;
    can_filter_st.FilterIdLow = 0x0000;
    can_filter_st.FilterMaskIdHigh = 0x0000;
    can_filter_st.FilterMaskIdLow = 0x0000;
    can_filter_st.FilterBank = 0;
    can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
    HAL_CAN_ConfigFilter(&hcan, &can_filter_st);
    HAL_CAN_Start(&hcan);
    HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING);

}

/**
  * @brief          hal库CAN回调函数,接收电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         Null
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	
	CAN_RxHeaderTypeDef rx_header;
	 uint8_t rx_data1[8];
		if (hcan->Instance == CAN1)
		{
		 HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data1);
		 if(hcan->Instance == CAN1)
			CAN_hook(&rx_header,rx_data1);
	  }
}

//统一处理can中断函数，并且记录发送数据的时间，作为离线判断依据
static void CAN_hook(CAN_RxHeaderTypeDef *rx_message , uint8_t* data)
{

switch (rx_message->StdId)
    {
			// case CAN1_3508_LF_ID:
			// {
			// 	get_motor_measure(&motor[LF],data);
			// 	break;
			// }
			// case CAN1_3508_RF_ID:
			// {
			// 	get_motor_measure(&motor[RF],data);
			// 	break;
			// }
			// case CAN1_3508_LB_ID:
			// {
			// 	get_motor_measure(&motor[LB],data);
			// 	break;
			// }
			// case CAN1_3508_RB_ID:
			// {
			// 	get_motor_measure(&motor[RB],data);
			// 	break;
			// }
            case mpu6050_ID:
            {
                // get_mpu6050_measure();
            }
			default:
			{
				break;
			}
	}
 
}
//统一处理can中断函数，并且记录发送数据的时间，作为离线判断依据

/*********************通用********************/
/**
  * @brief          返回超级电容数据
  * @param[in]      Null
  * @retval         Null
  */
const Chassis_Power_t *get_cap_measure_point(void)
{
    return &cap_measure;
}

const mpu6050_send_data_t *get_mpu6050_measure_point(void)
{
    return &mpu6050_send_data;

}
/**
  * @brief          返回超级电容数据
  * @param[in]      Null
  * @retval         Null
  */
const robofuture_cap_measure_t *get_robo_cap_measure_point(void)
{
    return &robo_cap_measure;
}

/**
  * @brief          返回电机对应地址
  * @param[in]      Null
  * @retval         Null
  */
// const motor_t *get_motor_measure_class(uint16_t type)
// {
// 	return &motor[type];
// }

/*************上下双板时使用**************/
/**
  * @brief          返回云台对底盘命令
  * @param[in]      Null
  * @retval         Null
  */
const chassis_order_measure_t *get_Gimbal_Oreder()
{
	return &chassis_order_measure;
}

/**
  * @brief          返回是否可以打弹
  * @param[in]      Null
  * @retval         电机数据指针
  */
const chassis_to_gimbal_t  *get_if_revolver_point(void)
{
    return &revolver_limit;
}

/**
  * @brief          返回UI数据
  * @param[in]      Null
  * @retval         Null
  */
const UI_order_to_chassis *get_UI_measure()
{
	return &UI_Measure;
}



/**
  * @apply          can1_motor1:LF  can1_motor2:RF can1_motor3:LB can1_motor4:RB
  * @brief          发送电机控制电流（控制CAN1 ID:1-4）
  * @param[in]      can1_motor1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can1_motor2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can1_motor3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can1_motor4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         Null
  */
void CAN1_200_cmd_motor(int16_t can1_motor1, int16_t can1_motor2, int16_t can1_motor3, int16_t can1_motor4)
{
    uint32_t send_mail_box;
    CAN1_200_tx_message.StdId = 0x200;
    CAN1_200_tx_message.IDE = CAN_ID_STD;
    CAN1_200_tx_message.RTR = CAN_RTR_DATA;
    CAN1_200_tx_message.DLC = 0x08;
    CAN1_200_send_data[0] = (can1_motor1 >> 8);
    CAN1_200_send_data[1] = can1_motor1;
    CAN1_200_send_data[2] = (can1_motor2 >> 8);
    CAN1_200_send_data[3] = can1_motor2;
    CAN1_200_send_data[4] = (can1_motor3 >> 8);
    CAN1_200_send_data[5] = can1_motor3;
    CAN1_200_send_data[6] = (can1_motor4 >> 8);
    CAN1_200_send_data[7] = can1_motor4;
    HAL_CAN_AddTxMessage(&hcan, &CAN1_200_tx_message, CAN1_200_send_data, &send_mail_box);
}


/**
  * @apply          can1_motor5:YAW  can1_motor6:PITCH
  * @brief          发送电机控制电流（控制CAN1 ID:5-8）
  * @param[in]      can1_motor5: (0x205) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can1_motor6: (0x206) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can1_motor7: (0x207) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can1_motor8: (0x208) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         Null
  */
void CAN1_1FF_cmd_motor(int16_t can1_motor5, int16_t can1_motor6, int16_t can1_motor7, int16_t can1_motor8)
{
    uint32_t send_mail_box;
    CAN1_1FF_tx_message.StdId = 0x1FF;
    CAN1_1FF_tx_message.IDE = CAN_ID_STD;
    CAN1_1FF_tx_message.RTR = CAN_RTR_DATA;
    CAN1_1FF_tx_message.DLC = 0x08;
    CAN1_1FF_send_data[0] = (can1_motor5 >> 8);
    CAN1_1FF_send_data[1] = can1_motor5;
    CAN1_1FF_send_data[2] = (can1_motor6 >> 8);
    CAN1_1FF_send_data[3] = can1_motor6;
    CAN1_1FF_send_data[4] = (can1_motor7 >> 8);
    CAN1_1FF_send_data[5] = can1_motor7;
    CAN1_1FF_send_data[6] = (can1_motor8 >> 8);
    CAN1_1FF_send_data[7] = can1_motor8;
    HAL_CAN_AddTxMessage(&hcan, &CAN1_1FF_tx_message, CAN1_1FF_send_data, &send_mail_box);
}


/**
  * @apply          can2_motor1:FireL  can2_motor2:FireR
  * @brief          发送电机控制电流（控制CAN2 ID:1-4）
  * @param[in]      can2_motor1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can2_motor2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can2_motor3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can2_motor4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         Null
  */
void CAN2_200_cmd_motor(int16_t can2_motor1, int16_t can2_motor2, int16_t can2_motor3, int16_t can2_motor4)
{
    uint32_t send_mail_box;
    CAN2_200_tx_message.StdId = 0x200;
    CAN2_200_tx_message.IDE = CAN_ID_STD;
    CAN2_200_tx_message.RTR = CAN_RTR_DATA;
    CAN2_200_tx_message.DLC = 0x08;
    CAN2_200_send_data[0] = (can2_motor1 >> 8);
    CAN2_200_send_data[1] = can2_motor1;
    CAN2_200_send_data[2] = (can2_motor2 >> 8);
    CAN2_200_send_data[3] = can2_motor2;
    CAN2_200_send_data[4] = (can2_motor3 >> 8);
    CAN2_200_send_data[5] = can2_motor3;
    CAN2_200_send_data[6] = (can2_motor4 >> 8);
    CAN2_200_send_data[7] = can2_motor4;
    HAL_CAN_AddTxMessage(&hcan, &CAN2_200_tx_message, CAN2_200_send_data, &send_mail_box);
}

//c
/**
  * @apply          暂未使用
  * @brief          发送电机控制电流（控制CAN2 ID:5-8）
  * @param[in]      can2_motor5: (0x205) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can2_motor6: (0x206) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can2_motor7: (0x207) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      can2_motor8: (0x208) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         Null
  */
void CAN2_1FF_cmd_motor(int16_t can2_motor5, int16_t can2_motor6, int16_t can2_motor7, int16_t can2_motor8)
{
    uint32_t send_mail_box;
    CAN2_1FF_tx_message.StdId = 0x1FF;
    CAN2_1FF_tx_message.IDE = CAN_ID_STD;
    CAN2_1FF_tx_message.RTR = CAN_RTR_DATA;
    CAN2_1FF_tx_message.DLC = 0x08;
    CAN2_1FF_send_data[0] = (can2_motor5 >> 8);
    CAN2_1FF_send_data[1] = can2_motor5;
    CAN2_1FF_send_data[2] = (can2_motor6 >> 8);
    CAN2_1FF_send_data[3] = can2_motor6;
    CAN2_1FF_send_data[4] = (can2_motor7 >> 8);
    CAN2_1FF_send_data[5] = can2_motor7;
    CAN2_1FF_send_data[6] = (can2_motor8 >> 8);
    CAN2_1FF_send_data[7] = can2_motor8;
    HAL_CAN_AddTxMessage(&hcan, &CAN2_1FF_tx_message, CAN2_1FF_send_data, &send_mail_box);
}

/**
  * @brief          发送ID为0x700的CAN包,它会设置3508电机进入快速设置ID
  * @param[in]      Null
  * @retval         Null
  */
void CAN_cmd_chassis_reset_ID(void)
{
    uint32_t send_mail_box;
    Reset_tx_message.StdId = 0x700;
    Reset_tx_message.IDE = CAN_ID_STD;
    Reset_tx_message.RTR = CAN_RTR_DATA;
    Reset_tx_message.DLC = 0x08;
    Reset_tx_send_data[0] = 0;
    Reset_tx_send_data[1] = 0;
    Reset_tx_send_data[2] = 0;
    Reset_tx_send_data[3] = 0;
    Reset_tx_send_data[4] = 0;
    Reset_tx_send_data[5] = 0;
    Reset_tx_send_data[6] = 0;
    Reset_tx_send_data[7] = 0;

    HAL_CAN_AddTxMessage(&hcan, &Reset_tx_message, Reset_tx_send_data, &send_mail_box);
}


/***********************************双板控制传信息用***************************************************/
//以下皆是
/**
  * @brief          控制电机并且发送wz信息
  * @param[in]      Null
  * @retval         Null
  */
void CAN1_1FF_cmd_motor_send_wz_info(int16_t can1_motor5, int16_t can1_motor6, fp32 info)
{
	uint32_t send_mail_box;
	uint16_t i =0;
  CAN1_1FF_tx_message.StdId = 0x1FF;
  CAN1_1FF_tx_message.IDE = CAN_ID_STD;
  CAN1_1FF_tx_message.RTR = CAN_RTR_DATA;
  CAN1_1FF_tx_message.DLC = 0x08;
	
	chassis_order_wz.f = info;
	
  CAN1_1FF_send_data[0] = (can1_motor5 >> 8);
  CAN1_1FF_send_data[1] = can1_motor5;
  CAN1_1FF_send_data[2] = (can1_motor6 >> 8);
  CAN1_1FF_send_data[3] = can1_motor6;

	for(i=0;i<4;i++)
	{
		CAN1_1FF_send_data[i+4] = chassis_order_wz.s[i];
	}
  HAL_CAN_AddTxMessage(&hcan, &CAN1_1FF_tx_message, CAN1_1FF_send_data, &send_mail_box);
}

/**
  * @brief          发送vx_set,vy_set信息
  * @param[in]      Null
  * @retval         Null
  */
void CAN1_301_send_vxvy_info(fp32 vx_set, fp32 vy_set)
{
	uint32_t send_mail_box;
	uint16_t i =0;
  CAN1_301_tx_message.StdId = 0x301;
  CAN1_301_tx_message.IDE = CAN_ID_STD;
  CAN1_301_tx_message.RTR = CAN_RTR_DATA;
  CAN1_301_tx_message.DLC = 0x08;
	
	chassis_order_vx.f = vx_set;
	chassis_order_vy.f = vy_set;
	
	for(i=0;i<4;i++)
  {
     CAN1_301_send_data[i]= chassis_order_vx.s[i];
		 CAN1_301_send_data[i+4]= chassis_order_vy.s[i];
  }
	
	HAL_CAN_AddTxMessage(&hcan, &CAN1_301_tx_message, CAN1_301_send_data, &send_mail_box);
}

/**
  * @brief          CAN1发送状态信息
  * @param[in]      Null
  * @retval         Null
  */
void CAN1_send_state_info(uint8_t reset,uint8_t Auto, uint8_t Spin ,uint8_t IfShift ,uint8_t OBLI)
{
	uint32_t send_mail_box;

  CAN1_3FF_tx_message.StdId = 0x3FF;
  CAN1_3FF_tx_message.IDE = CAN_ID_STD;
  CAN1_3FF_tx_message.RTR = CAN_RTR_DATA;
  CAN1_3FF_tx_message.DLC = 0x08;
	
	CAN1_3FF_send_data[0] = reset;
//pitch.f = Pitch;
//	
//for(i=0;i<4;i++)
//{
//  chassis_yaw_pitch[i]= pitch.s[i];
////chassis_yaw_pitch[i+4]= pitch.s[i];
//}
	CAN1_3FF_send_data[4] = Auto;
	CAN1_3FF_send_data[5] = Spin;
	CAN1_3FF_send_data[6] = IfShift;
	CAN1_3FF_send_data[7] = OBLI;
		
  HAL_CAN_AddTxMessage(&hcan, &CAN1_3FF_tx_message, CAN1_3FF_send_data, &send_mail_box);
	
}


void CAN_CMD_ROBO_CAP(uint8_t dischargecmd,uint8_t ctrlmode,uint16_t chassis_power_buff,uint16_t chassis_power_buff_limit,uint16_t chassis_power_limit)
{
	  uint32_t send_mail_box;
    chassis_tx_message.StdId = 0x100;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;

		chassis_can_send_data[0]=dischargecmd;
		chassis_can_send_data[1]=ctrlmode;
		chassis_can_send_data[2]=chassis_power_buff >> 8;
		chassis_can_send_data[3]=chassis_power_buff;
    chassis_can_send_data[4] = chassis_power_buff_limit >> 8;
    chassis_can_send_data[5] = chassis_power_buff_limit;
		chassis_can_send_data[6] = chassis_power_limit >> 8;
    chassis_can_send_data[7] = chassis_power_limit;
    
		HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
} 

//发送mpu6050数据
void CAN_send_mpu6050_measure(fp32 Accel_pitch,fp32 Accel_roll,fp32 Pitch_angle,fp32 Roll_angle)
{
    uint32_t send_mail_box;
    uint16_t i = 0;
    CAN1_208_tx_message.StdId = 0x208;
    CAN1_208_tx_message.IDE = CAN_ID_STD;
    CAN1_208_tx_message.RTR = CAN_RTR_DATA;
    CAN1_208_tx_message.DLC = 0x08;

    mpu6050_send_data.accel_pitch.f = Accel_pitch;
    mpu6050_send_data.accel_roll.f = Accel_roll;
	for(i=0;i<4;i++)
	{
		CAN1_208_send_data[i] = mpu6050_send_data.accel_pitch.s[i];
        CAN1_208_send_data[i+4] = mpu6050_send_data.accel_roll.s[i];
	}

	HAL_CAN_AddTxMessage(&hcan, &CAN1_208_tx_message, CAN1_208_send_data, &send_mail_box);

}

